package com.hrg.rds.infra.executor.impl;

import cn.hutool.core.collection.CollUtil;
import cn.hutool.core.util.IdUtil;
import cn.hutool.core.util.NumberUtil;
import cn.hutool.json.JSONUtil;
import com.hrg.rds.biz.comm.NavService;
import com.hrg.rds.biz.comm.StatusService;
import com.hrg.rds.biz.config.RdsConfiguration;
import com.hrg.rds.biz.robot.domain.AgvVo;
import com.hrg.rds.biz.traffic.NavType;
import com.hrg.rds.biz.traffic.NavVo;
import com.hrg.rds.biz.traffic.TrafficManagement;
import com.hrg.rds.biz.traffic.TrafficZoneVo;
import com.hrg.rds.biz.waybill.domain.instance.AgvMsg;
import com.hrg.rds.biz.waybill.domain.instance.WaybillInstanceVo;
import com.hrg.rds.biz.waybill.domain.template.cmd.CmdParam;
import com.hrg.rds.biz.waybill.service.AgvMsgService;
import com.hrg.rds.infra.executor.CmdExecutor;
import com.hrg.rds.infra.executor.CmdInstanceContext;
import com.hrg.rds.infra.executor.CmdInstanceResult;
import com.hrg.seer.sdk.rbk.msg.nav.BaseNav;
import com.hrg.seer.sdk.rbk.msg.nav.GoTargetListReq;
import com.hrg.seer.sdk.rbk.msg.nav.Nav;
import com.hrg.seer.sdk.rbk.msg.status.Task;
import com.hrg.seer.sdk.rbk.msg.status.TaskStatusPackage;
import com.hrg.sys.common.constant.UserConstants;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;

import java.time.Duration;
import java.util.*;
import java.util.function.Function;
import java.util.stream.Collectors;
/*
1. Robokit 3.3 以上版本必须包含三个元素： source_id 表示线路的起点， id 表示线路的终点， task_id 表示这个任务的唯一编号。
2. source_id 和 id 之间必须有直接相连的线路，不可以跳点，否则机器人会报错。
3. id 如果不是 SELF_POSITION ， source_id 不能是 SELF_POSITION 。//
4.  id 和 source_id 可以同时为 SELF_POSITION 。//id为SELF_POSITION时，机器人在原地执行operation
5. 此 API 需要强制指定 task_id 。
6. task_id 不能重复。
7. 如果当前机器人在执行一个任务，那么新任务会自动追加在当前任务的后面。如果当前任务失败、或者被取消，新任务也会被取消。如果当前任务完成， 那么会按接收顺序执行新任务。
8. 指定路径导航也支持导航任务中带有 operation，执行脚本，具体参见 固定路径导航 。
9. 如果机器人在执行指定路径导航任务，追加新任务时，新任务的线路和最后一个任务的线路不是相连，（比如最后一个任务的路径终点和新任务的路径的起点不是同一个点），而是跳跃的。
	如果跳跃的距离大于 OutPathDist ，会导致线路外错误，否则直接导航到不相连的线路。
*/

@Slf4j
@Service("AgvNavCmd")
public class AgvNavCmdExecutor implements CmdExecutor {
	@Autowired
	private AgvMsgService agvMsgService;

	@Override
	public Duration getRetryInterval() {
		return Duration.ofSeconds(1);
	}

	@Override
	public CmdInstanceResult execute(List<CmdParam> params, CmdInstanceContext cmdInstanceContext) {
		log.info("AgvNavCmdExecutor execute, cmdId = "+ cmdInstanceContext.getCmdId());
		try {
			AgvVo agvVo = cmdInstanceContext.getAgvVo();
			if (!agvVo.isIfAvailable4Current()) {
				log.info("agv[{}] is not available", agvVo.getIp());
				return CmdInstanceResult.retry();
			}

			Map<String, CmdParam> map = params.stream().collect(Collectors.toMap(CmdParam::getKey, Function.identity()));
			String targetStation = map.get("targetStation").getEvaluatedValue();
			// 是否只占用到当前站点
			boolean onlyOccupiedToCurrent = false;
			if (map.containsKey("onlyOccupiedToCurrent")) {
				onlyOccupiedToCurrent = map.get("onlyOccupiedToCurrent").getBooleanEvaluatedValue();
			}
			// 检查是否已经发送过导航命令
			AgvMsg agvMsg = cmdInstanceContext.getLastAgvMsg();
			if (agvMsg != null) {
				List<Nav> navs = JSONUtil.toList(agvMsg.getMsg(), Nav.class);
				List<String> taskIds = navs.stream().map(Nav::getTask_id).collect(Collectors.toList());
				// 查看发送的命令是否被agv执行完成；
				TaskStatusPackage taskStatusPackage = StatusService.getTaskStatus(agvVo.getIp(), taskIds);
				if (taskStatusPackage == null || CollUtil.isEmpty(taskStatusPackage.getTask_status_list())) {
					return CmdInstanceResult.retry();
				}
				Double distance = taskStatusPackage.getDistance();
				List<Task> taskStatus = taskStatusPackage.getTask_status_list();
				// 纯路径导航，successByDistance|successByStation 可以设置离目标点还剩余几个点认为执行成功，默认1个点；或者设置离目标点还剩多少距离时认为执行成功，默认1m
				// 故障处理：人工移动到目标点
				int uncompletedCnt = 0, completedCnt = 0, failedCnt = 0;
				for (Task task : taskStatus) {
					// 按照任务执行先后顺序存储
					Integer status = task.getStatus();
					if (status < 4) {
						// 未完成,uncompleted
						uncompletedCnt++;
					} else if (status == 4) {
						// 执行完成,completed
						completedCnt++;
					} else {
						// 执行失败,failed
						failedCnt++;
					}
				}
				// 如果 successByDistance 和 successByStation 两个同时配置，两个是或的关系，即任意一个条件满足就成功了
				if (map.containsKey("successByDistance")) {
					/*2.6 根据实际需要修改 OpenDoorDist 参数；（当机器人距离门 OpenDoorDist 的距离时，RdsCore 对门控模块发出开门请求）
					注意：此参数需要根据具体实际进行修改，默认为10m，即当机器人距离门 10m 时，RdsCore 对门控模块发出开门请求；*/
					
					// 配置目标站点多远认为执行成功
					int successByDistance = map.get("successByDistance").getIntegerEvaluatedValue();
					if (NumberUtil.compare(distance, successByDistance) <= 0) {
						// 距离
						log.info("到达目标站点{}前距离{}个站点{}", targetStation, distance, agvVo.getCurrentStation());
						return CmdInstanceResult.success();
					}
				}
				if (map.containsKey("successByStation")) {
					// 配置目标站点前几个点认为执行成功
					int successByStation = map.get("successByStation").getIntegerEvaluatedValue();
					if (uncompletedCnt <= successByStation) {
						// 数量
						log.info("到达目标站点{}前第{}个站点{}", targetStation, uncompletedCnt, agvVo.getCurrentStation());
						return CmdInstanceResult.success();
					}
				}
				// 默认到达站点成功
				if (completedCnt == taskIds.size()) {
					// 完全到达站点
					log.info("到达目标站点{}", targetStation);
					return CmdInstanceResult.success();
				} else if (completedCnt + uncompletedCnt == taskIds.size()) {
					return CmdInstanceResult.retry();
				} else {
					// failedCnt > 0
					// 重新发送导航命令
					log.info("导航失败，重新发送导航命令，failedCnt = {}", failedCnt);
					sendAndSaveAgvMsg(agvVo, cmdInstanceContext, targetStation, onlyOccupiedToCurrent);
					return CmdInstanceResult.retry();
				}
			} else {
				/*if (agvVo.getCurrentStation().equals(targetStation)) {
					// 重新导航到当前点
					// 可能需要先到前置点然后再到当前点。查询相邻的点和车辆已完成点的交集，判断前置点，如果没有前置点无效直接成功；有前置点，先到前置点再到当前点。
					// 操作类的，更改为self_position
					return CmdInstanceResult.success();
				}*/
				sendAndSaveAgvMsg(agvVo, cmdInstanceContext, targetStation, onlyOccupiedToCurrent);
				return CmdInstanceResult.retry();
			}
		} catch (Exception e) {
			log.error("AgvNavCmdExecutor execute error", e);
			return CmdInstanceResult.retry(e.getMessage());
		}
	}

	private void sendAndSaveAgvMsg(AgvVo agvVo, CmdInstanceContext cmdInstanceContext, String targetStation, boolean onlyOccupiedToCurrent) {
		// 获取导航路径
		List<String> paths = NavService.getNavPath(agvVo.getIp(), agvVo.getCurrentStation(), targetStation);
		if (CollUtil.isEmpty(paths)) {
			return;
		}
		// 检查交管是否满足
		if (!TrafficManagement.tryTraffic(onlyOccupiedToCurrent, cmdInstanceContext, agvVo, targetStation, paths)) return;

		// 发送导航命令 // 可以记录task_id，但每次重新发送重新生成task_id
		List<BaseNav> navs = new ArrayList<>();
		for (int i = 1; i < paths.size(); i++) {
			Nav nav = new Nav();
			nav.setId(paths.get(i));
			nav.setSource_id(paths.get(i - 1));
			nav.setTask_id(IdUtil.getSnowflakeNextIdStr());
			navs.add(nav);
		}
		boolean success = NavService.goTargetList(agvVo.getIp(), navs);
		// 发送导航命令成功
		if (success) {
			// 记录导航命令
			AgvMsg agvMsg = new AgvMsg();
			agvMsg.setId(IdUtil.getSnowflakeNextIdStr());
			agvMsg.setCmdInstanceId(cmdInstanceContext.getCmdInstanceId());
			agvMsg.setWaybillInstanceId(cmdInstanceContext.getWaybillInstanceId());
			agvMsg.setMsg(JSONUtil.toJsonStr(navs));
			Date now = new Date();
			agvMsg.setCreateTime(now);
			agvMsg.setCreateBy(UserConstants.SYS_USER);
			agvMsg.setUpdateTime(now);
			agvMsg.setUpdateBy(UserConstants.SYS_USER);
			agvMsgService.insert(agvMsg);
			cmdInstanceContext.setLastAgvMsg(agvMsg);
		}
	}
}
